/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Collision detector
 */

#ifndef COLLISION_DETECTOR_H_
#define COLLISION_DETECTOR_H_

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"

#define PI                  3.141592654
#define COLLISION_THRESHOLD 3

struct LaserSample
{
    double angle;
    double distance;
};

class CollisionDetector
{
public:
    CollisionDetector();
    ~CollisionDetector(){}

    int calculateSamples(const sensor_msgs::LaserScan::ConstPtr& scan);
    void readScan(const sensor_msgs::LaserScan::ConstPtr& scan);
    bool safeToMoveForward(group6::safeToMoveForward::Request &req, group6::safeToMoveForward::Response &res);
    bool readRobotParameters();

    bool collision() { return mCollision; }
    void clearCollision() { mCollision = false; }

    double getLaserPosX() { return mLaserPosX; }
    double getLaserPosY() { return mLaserPosY; }
    double getChassisPosX() { return mChassisPosX; }
    double getChassisPosY() { return mChassisPosY; }
    double getManipulatorPosX() { return mManipulatorPosX; }
    double getManipulatorPosY() { return mManipulatorPosY; }
    double getChassisWidth() { return mChassisWidth; }
    double getClearanceX() { return mClearanceX; }
    double getClearanceY() { return mClearanceY; }
    
    double getLaserToCollisionX() { return mLaserToCollisionX; }
    double getLaserToCollisionY() { return mLaserToCollisionY; }

private:
    // ROS handles
    ros::NodeHandle     mNode;
    ros::Subscriber     mLaserScanSubscriber;
    ros::ServiceServer  mSafeToMoveService;
    ros::Publisher      mObstacleApproachingPublisher;

    // State
    bool    mCollision;

    // Robot parameters
    double  mLaserPosX;
    double  mLaserPosY;
    double  mChassisPosX;
    double  mChassisPosY;
    double  mManipulatorPosX;
    double  mManipulatorPosY;
    double  mChassisWidth;
    double  mClearanceX;
    double  mClearanceY;

    // Calculated parameters
    double  mLaserToCollisionX;
    double  mLaserToCollisionY;
};

#endif